#include "spring_factory.h"

#include "particle.h"
#include "world.h"

SpringBody* SpringFactory::makeBox(World *world, real x, real y, real w, real h)
{
	SpringBody *body = world->createBody();
	body->name = "box";
	Particle *v1 = world->createParticle(x,y);
	Particle *v2 = world->createParticle(x+w,y);
	Particle *v3 = world->createParticle(x+w,y+h);
	Particle *v4 = world->createParticle(x,y+h);
	Edge *e1 = world->createEdge(v1,v2);
	Edge *e2 = world->createEdge(v2,v3);
	Edge *e3 = world->createEdge(v3,v4);
	Edge *e4 = world->createEdge(v4,v1);

	// Crossbars for stability
	Edge *e5 = world->createEdge(v1,v3);
	Edge *e6 = world->createEdge(v2,v4);
	e5->isBoundary = false;
	e6->isBoundary = false;

	body->addParticle( v1 );
	body->addParticle( v2 );
	body->addParticle( v3 );
	body->addParticle( v4 );

	body->addEdge( e1 );
	body->addEdge( e2 );
	body->addEdge( e3 );
	body->addEdge( e4 );

	body->addEdge( e5 );
	body->addEdge( e6 );

	body->isAlive = true;
	body->calcCenter();
	real boxW = body->bounds.xmax - body->bounds.xmin;
	real boxH = body->bounds.ymax - body->bounds.ymin;
	body->obb = OBB2D( body->centroid, boxW, boxH, 0);
	return body;
}


SpringBody* SpringFactory::makeSphere(World *world, real x, real y, real radius, bool locked )
{
	SpringBody *body = world->createBody();
	body->name = "circle";

	Particle *pCenter = world->createParticle(x,y);
	body->addParticle( pCenter );
	// Lock center particle
	if( locked ) pCenter->massInverse = 0;

	std::vector<Particle*> vertexList;
	// Create particles in a circle
	int segments = 16;
	for(int i=1; i<segments; i++)
	{
		real px = (real)(x + cos( i*2.0*M_PI/(segments-1) ) * radius);
		real py = (real)(y + sin( i*2.0*M_PI/(segments-1) ) * radius);
		Particle *p = world->createParticle(px,py);
		body->addParticle( p );
		vertexList.push_back( p );
	}

	for(size_t i=0; i<vertexList.size(); i++)
	{
		Edge *e = world->createEdge( pCenter, vertexList[i]);
		body->addEdge( e );
	}

	for(size_t i=0; i<vertexList.size()-1; i++)
	{
		Particle *p1 = vertexList[i];
		Particle *p2 = vertexList[i+1];
		Edge *e = world->createEdge( p1, p2);
		body->addEdge( e );
	}
	Particle *p1 = vertexList[0];
	Particle *p2 = vertexList[ vertexList.size()-1 ];
	Edge *e = world->createEdge( p1, p2);
	body->addEdge( e );
	body->isAlive = true;
	return body;
}

void SpringFactory::makeDebris( World *world, SpringBody *inputBody, std::vector<SpringBody*> &fractureList )
{
	Vec2f fracturePoint(0,0);
	for each(Particle *p in inputBody->particles)
	{
		fracturePoint += p->pos;
	}
	fracturePoint /= (real)inputBody->particles.size();

	inputBody->calcBoundryEdges();
	for each(Edge *e in inputBody->edges)
	{
		if( e->isBoundary )
		{
			SpringBody *splint = world->createBody();
			splint->name = "splint";
			Particle *p1 = world->createParticle(e->particles[0]->pos.x(), e->particles[0]->pos.y() );
			Particle *p2 = world->createParticle(e->particles[1]->pos.x(), e->particles[1]->pos.y() );
			Particle *p3 = world->createParticle(fracturePoint.x(), fracturePoint.y() );
			splint->addParticle(p1);
			splint->addParticle(p2);
			splint->addParticle(p3);
			splint->addEdge( world->createEdge(p1,p2) );
			splint->addEdge( world->createEdge(p2,p3) );
			splint->addEdge( world->createEdge(p1,p3) );
			splint->isAlive = true;
			fractureList.push_back(splint);
		}

	}
}

SpringBody* SpringFactory::makeTriangle(World *world, real x, real y, real size )
{
	SpringBody *body = world->createBody();
	body->name = "Triangle";
	Particle *p1 = world->createParticle(x,y+size/2);
	Particle *p2 = world->createParticle(x-size/2,y-size/2);
	Particle *p3 = world->createParticle(x+size/2,y-size/2);
	Edge *e1 = world->createEdge( p1,p2 );
	Edge *e2 = world->createEdge( p2,p3 );
	Edge *e3 = world->createEdge( p1,p3 );
	body->addParticle(p1);
	body->addParticle(p2);
	body->addParticle(p3);
	body->addEdge(e1);
	body->addEdge(e2);
	body->addEdge(e3);
	body->isAlive = true;
	return body;
}

SpringBody* SpringFactory::makeGear( World *world, real x, real y, real size )
{
	bool locked = true;
	real radius = size;
	SpringBody *body = world->createBody();
	body->name = "makeGear";

	Particle *pCenter = world->createParticle(x,y);
	body->addParticle( pCenter );
	// Lock center particle
	if( locked ) pCenter->massInverse = 0;

	std::vector<Particle*> vertexList;
	// Create particles in a circle
	int segments = 12;
	for(int i=1; i<segments; i++)
	{
		real px = (real)(x + cos( real(i)*2.0*M_PI/(segments-1) ) * radius);
		real py = (real)(y + sin( real(i)*2.0*M_PI/(segments-1) ) * radius);
		Particle *p = world->createParticle(px,py);
		body->addParticle( p );
		vertexList.push_back( p );
	}
//for(i=NUM-1,j=0;j<NUM;i=j,j++)

	//for(size_t i=0; i<vertexList.size()-1; i+=2)
	
	//size_t j=0; j<vertexList.size(); i=j, j++

	size_t lastIdx = vertexList.size()-1;

	for(size_t i=lastIdx, j=0; j<vertexList.size(); i=j, j++)
	{
		Particle *p1 = vertexList[i];
		Particle *p2 = vertexList[j];
		Vec2f p1p2 = p2->pos - p1->pos;
		Vec2f normal( -p1p2.y(), p1p2.x() );
		normal = normalize(normal);
		Vec2f p3Pos = (p1->pos + p1p2 * 0.5) + normal * 1*length(p1p2);
		Particle *p3 = world->createParticle(p3Pos[0], p3Pos[1]);
		
		SpringBody *bodyPart = world->createBody();
		bodyPart->name = "gearTooth";
		Edge *e1 = world->createEdge( p1,p2 );
		Edge *e2 = world->createEdge( p2,p3 );
		Edge *e3 = world->createEdge( p1,p3 );
		bodyPart->addParticle(p1);
		bodyPart->addParticle(p2);
		bodyPart->addParticle(p3);
		bodyPart->addEdge(e1);
		bodyPart->addEdge(e2);
		bodyPart->addEdge(e3);
		bodyPart->isAlive = true;

	}


	for(size_t i=0; i<vertexList.size(); i++)
	{
		Edge *e = world->createEdge( pCenter, vertexList[i]);
		body->addEdge( e );
	}

	for(size_t i=0; i<vertexList.size()-1; i++)
	{
		Particle *p1 = vertexList[i];
		Particle *p2 = vertexList[i+1];
		Edge *e = world->createEdge( p1, p2);
		body->addEdge( e );
	}
	Particle *p1 = vertexList[0];
	Particle *p2 = vertexList[ vertexList.size()-1 ];
	Edge *e = world->createEdge( p1, p2);
	body->addEdge( e );
	body->isAlive = true;
	return body;
}
